﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace NatNetClient
{
    public class OptiTrackPacket : IPacket
    {
        private readonly short _lenght;
        private readonly short _type;
        private readonly short _x;
        private readonly short _y;
        private readonly short _z;
        private readonly short _qx;
        private readonly short _qy;
        private readonly short _qz;
        private readonly short _qw;
        private readonly short _meanError;
        private readonly ushort _timeStamp;
        private ushort _crc16;

        public OptiTrackPacket(short x,short y, short z, short qx,short qy,short qz,short qw,short error, ushort timeStamp, ushort packetCount)
        {
            _type = 5;
            _lenght = 24; 
            // pacchetto è lungo 24 byte

            _x = x;
            _y = y;
            _z = z;
            _qw = qw;
            _qx = qx;
            _qz = qz;
            _qy = qy;
            _meanError = error;
            _timeStamp = timeStamp;


            //15 NOV 2011 change of reference 
            //NED frame, north is KG
            _x = (short)-z;
            _y = x;
            _z = (short)-y;

            _qw = qw;
            _qx = (short)-qz;
            _qy = qx;
            _qz = (short)-qy;

            //_qw = (short)-qw;
            //_qx = qz;
            //_qy = (short)-qx;
            //_qz = qy;

            //////////////////////





            //calcolo del CRC16
            string temp = String.Format("S {0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} E",
                _lenght, _type, _x, _y, _z, _qw, _qx, _qy, _qz, _meanError, _timeStamp);
            Crc16 crc = new Crc16();
            _crc16 = packetCount; //crc.ComputeChecksum(UTF8Encoding.ASCII.GetBytes(temp));
        }

        public override string ToString()
        {
            //_lenght = 24;
            string PH = "LENGHT";
            string packet = String.Format("S {0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} E",
                PH, _type, _x, _y, _z, _qw, _qx, _qy, _qz, _meanError, _timeStamp, _crc16);

            int len = packet.Length;
            int diff = PH.Length;
            int ll = String.Format("{0}", len).Length;
            diff = ll - diff;
            len += diff;
            diff = ll;
            ll = String.Format("{0}", len).Length;
            diff = ll - diff;
            len += diff;
            packet = packet.Replace(PH, String.Format("{0}", len));
            return packet;
        }

        public byte[] ToBinary()
        {
            //_lenght = 24;
            byte[] p = new byte[29];
            //HEADER
            p[0] = 0xFF;			//put the header information	
            p[1] = 0xFF;			//4 bytes	
            p[2] = 0xFF;			//Last byte defines the mask: 0xF0
            p[3] = 0xF2;            //16 bit fields
            //LENGHT
            p[4] = 0x00;
            p = Binary.Write2BValue(p.Length, 5, p);
            //TYPE PACKET
            p[6] = 0x00;
            p[7] = 0x05;
            //PACKET DATA
            p = Binary.Write2BValue(_x, 8, p);
            p = Binary.Write2BValue(_y, 10, p);
            p = Binary.Write2BValue(_z, 12, p);

            p = Binary.Write2BValue(_qx, 14, p);
            p = Binary.Write2BValue(_qy, 16, p);
            p = Binary.Write2BValue(_qz, 18, p);
            p = Binary.Write2BValue(_qw, 20, p);

            p = Binary.Write2BValue(_meanError, 22, p);
            p = Binary.Write2BValue(_timeStamp, 24, p);
            p = Binary.Write2BValue(_crc16, 26, p);

            //FOOTER

            p[28] = 0xFF;
            p[29] = 0xFF;
            return p;
        }

        public byte[] ToMavlink()
        {
            byte[] packet = new byte[0];
            MAVLink mav = new MAVLink();
            MAVLink.mavlink_vicon_position_estimate_t p = new MAVLink.mavlink_vicon_position_estimate_t();
            p.roll = 0;
            p.pitch = 0;
            p.yaw = 0;
            p.x = _x;
            p.y = _y;
            p.z = _z;
            packet = MavlinkUtil.StructureToByteArray(p);
            return packet;
        }
    }
}
